#ifndef VECTOR3_H
#define VECTOR3_H

#include "typedefs.h"
#include <Eigen/Core>
#include <QString>

namespace Math
{
    class Vector3
    {
        #ifdef USE_FLOAT
            Eigen::Vector3f v;
        #else
            Eigen::Vector3d v;
        #endif

        public:
        #ifdef USE_FLOAT
            Eigen::Vector3f getEigen() const {return v;}
        #else
            Eigen::Vector3d getEigen() const {return v;}
        #endif

            Vector3();

            Vector3(const Vector3 &a) { v = a.v;}

#ifdef USE_FLOAT
            Vector3(const Eigen::Vector3f &ev) { v = ev; }
#else
            Vector3(const Eigen::Vector3d &ev) { v = ev; }
#endif
            Vector3(const Real &x, const Real &y, const Real &z) { v[0] = x;v[1] = y;v[2] = z; }


            Vector3(bool zero) { if(zero) setValue(0);  }

            void set(const Real &x, const Real &y, const Real &z) { v[0] = x;v[1] = y;v[2] = z;}

            void setValue(const Real &val) { v[0] = val; v[1] = val; v[2] = val; }

            Real x(void) const { return v[0]; }
            Real y(void) const { return v[1]; }
            Real z(void) const { return v[2]; }

            void x(const Real &a) { v[0] = a; }
            void y(const Real &a) { v[1] = a; }
            void z(const Real &a) { v[2] = a; }

            //Vector3& operator=(const Vector3& b) { v = b.v; return *this;}

            Vector3 operator*(const Real& b)  const {
                return Vector3(v * b);
            }

            Vector3 operator-(const Vector3& b) const {
                return Vector3(v - b.getEigen());
            }

            void normalize() { v.normalize(); }

            Real getLength() const { return v.norm(); }

            Real getSquaredLength() const { return v.squaredNorm(); }

            QString toString() const {
                QString res;
                QString sx,sy,sz;
                sx.setNum(x());
                sy.setNum(y());
                sz.setNum(z());
                res = sx + " " + sy + " " + sz;
                return res;
            }

            static Vector3 fromString(const QString &s) {
                std::stringstream ss(s.toStdString());
                Real x,y,z;
                ss >> x >> y >> z;
                return Vector3(x,y,z);
            }

            Real operator[](const int &i) const { return v[i];}

            Real& operator[](const int &i)  { return v[i];}

            static Vector3 ZERO;

            static Vector3 UNIT_X;
            static Vector3 UNIT_Y;
            static Vector3 UNIT_Z;

            Vector3 operator+(const Vector3 &a) const { return Vector3(v+a.v); }

            void operator+=(const Vector3 &a) { v += a.v; }

            Vector3 operator*(const Real &a) { return Vector3(v*a); }

            void operator*=(const Real &a) { v *= a; }

            bool operator<(const Vector3 &a) const
            {
                if(x() < a.x()) return true;
                else if(a.x() < x()) return false;
                if(y() < a.y()) return true;
                else if(a.y() < y()) return false;
                if(z() < a.z()) return true;
                else if(a.z() < z()) return false;
                return false;
            }

            Vector3 operator-(){ return Vector3(v*-1.0); }

            static Vector3 crossProd(const Vector3 &a, const Vector3 &b) { return a.v.cross(b.v); }

            static Real dotProd(const Vector3 &a, const Vector3 &b) { return a.v.dot(b.v); }
    };
}



#endif // VECTOR3_H
